Intel RealSense D435iとSkeleton Tracking SDKを連携させたら簡単だったのでコードを読んでみた[Windows編]

せーのでございます。

先日、Intel RealSenseからCubemos製のSkeleton Tracking SDKがリリースされたので触ってみました。

Intel RealSenseに待望のSkeleton Tracking SDKが出た（cubemos製）ので触ってみた[Windows編]

今日は画像ファイルから骨格検知するのではなく、実際のRealSenseから読み込んで骨格検知してみたいと思います。

本記事では上記記事にて作成したプログラムを使用します。一緒に試してみたい方は上記記事をお読みになってプロジェクトを作成した上でお読み下さい

実はもうサンプルがあった

先日の触ってみた記事内にてCMAKEを使ってサンプルプロジェクトを作成しました（上記記事内「サンプルexeのソースを見てみる」参照）。

そのプロジェクトを開いてみると、中に「realsense」というプロジェクトがあります。

これを走らせるだけです。なんと簡単な。。。ではビルドして動かしてみます。

「Hold the Intel RealSense with person(s) in scene and and hit ...」

という表示が出たら、PCにRealSense D435iとつないで、人を写した状態にしてEnterキーを叩きます。

RealSenseから取得した情報を元に推論を行い、骨格検知の結果が出てきたら成功です。

いくつかのキーポイントが表示されていないのは、座った状態で上半身だけ写したからかと思います。

コードを読んでみる

あまりにあっさり終わってしまったので、コードを読んでみたいと思います。
SDKのインストールディレクトリ（先日の記事で環境変数「%CUBEMOS_SKEL_SDK%」で設定したパス）内にdocsというフォルダがあり、その中に開発者用のドキュメントがhtml形式で格納されています。

こちらを参考にコードを読みます。

コード全文はこのようになっています。

using System;
using Intel.RealSense;

// This sample shows how to use the cubemos Skeleton Tracking C# API in a console application
namespace Cubemos.Samples
{
    class Program {
        static void Main(string[] args)
        {

            Console.WriteLine("Initializing console Skeleton Tracking sample with RealSense ... ");

            // Initialize logging to output all messages with severity level INFO or higher to the console
            Cubemos.Api.InitialiseLogging(Cubemos.LogLevel.CM_LL_ERROR, bWriteToConsole : true);
            Cubemos.SkeletonTracking.Api skeletontrackingApi;

            // Create cubemos Skeleton tracking Api handle and specify the directory containing a cubemos_license.json file
            try
            {
                skeletontrackingApi = new Cubemos.SkeletonTracking.Api(Common.DefaultLicenseDir());
            }
            catch (Cubemos.Exception ex)
            {
                Console.WriteLine("If you haven't activated the SDK yet, please run post_installation script as described in the Getting Started Guide to activate your license.");
                Console.ReadLine();
                return;
            }

            // Initialise cubemos DNN framework with the required model
            try
            {
                skeletontrackingApi.LoadModel(Cubemos.TargetComputeDevice.CM_CPU,
                                              Common.DefaultModelDir() + "\\fp32\\skeleton-tracking.cubemos");
            }
            catch (Cubemos.Exception ex)
            {
                Console.WriteLine(String.Format("Error during model loading. " +
                      "Please verify the model exists at the path {0}. Details: {1}", Common.DefaultModelDir() + "\\fp32\\skeleton-tracking.cubemos", ex));
                Console.ReadLine();
                return;
            }

            Console.Write("Hold the Intel RealSense with person(s) in scene and hit <ENTER>... ");
            Console.ReadLine();

            // Initialise the intel realsense pipeline as an acquisition device
            Pipeline pipeline = new Pipeline();
            Config cfg = new Config();
            Context context = new Intel.RealSense.Context();
            cfg.EnableStream(Intel.RealSense.Stream.Color, 1280, 720, Format.Bgr8, framerate : 30);
            PipelineProfile pp = pipeline.Start(cfg);

            // Set the network input size to 128 for faster inference
            int networkHeight = 128;

            // Acquire a single color frame and run Skeleton Tracking on it
            using(var frames = pipeline.WaitForFrames())
            {
                var frame = frames.ColorFrame.DisposeWith(frames);
                System.Drawing.Bitmap inputImage =
                  new System.Drawing.Bitmap(frame.Width,
                                            frame.Height,
                                            frame.Stride,
                                            System.Drawing.Imaging.PixelFormat.Format24bppRgb,
                                            frame.Data);

                System.Collections.Generic.List<Cubemos.SkeletonTracking.Api.SkeletonKeypoints> skeletonKeypoints;

                // Send inference request and get the skeletons
                skeletontrackingApi.RunSkeletonTracking(ref inputImage, networkHeight, out skeletonKeypoints);

                // Output detected skeletons
                Console.WriteLine("# Persons detected: " + skeletonKeypoints.Count);
                for (int skeleton_index = 0; skeleton_index < skeletonKeypoints.Count; skeleton_index++) {
                    var skeleton = skeletonKeypoints[skeleton_index];
                    Console.WriteLine("Skeleton #" + skeleton_index);
                    for (int joint_index = 0; joint_index < skeleton.listJoints.Count; joint_index++) {
                        Cubemos.SkeletonTracking.Api.Coordinate coordinate = skeleton.listJoints[joint_index];
                        Console.WriteLine("\tJoint coordinate #" + joint_index + ": " + coordinate.x + "; " +
                                          coordinate.y);
                    }
                }
            }
            Console.Write("Press <Enter> to exit... ");
            Console.ReadLine();
        }
    }
}

では順番に見ていきましょう。

Initialise

 Cubemos.Api.InitialiseLogging(Cubemos.LogLevel.CM_LL_ERROR, bWriteToConsole : true);
 Cubemos.SkeletonTracking.Api skeletontrackingApi;

 try
            {
                skeletontrackingApi = new Cubemos.SkeletonTracking.Api(Common.DefaultLicenseDir());
            }
            catch (Cubemos.Exception ex)
            {
                Console.WriteLine("If you haven't activated the SDK yet, please run post_installation script as described in the Getting Started Guide to activate your license.");
                Console.ReadLine();
                return;
            }

最初のブロックではそれぞれのクラスやモジュールを作成して初期化しています。
Cubemos.Api.InitialiseLoggingはログの設定ですね。引数としてログレベル（ここではERRORの時のみ吐く）とコンソールにログを表示するかどうかを設定しています。
その後Skeleton Tracking APIをnewしています。引数としてインストール時に作成されたライセンスファイルを指定しています。プログラム上では「Common.cs」という静的クラスを作り、パス関係をまとめています。

LoadModel

            try
            {
                skeletontrackingApi.LoadModel(Cubemos.TargetComputeDevice.CM_CPU,
                                              Common.DefaultModelDir() + "\\fp32\\skeleton-tracking.cubemos");
            }
            catch (Cubemos.Exception ex)
            {
                Console.WriteLine(String.Format("Error during model loading. " +
                      "Please verify the model exists at the path {0}. Details: {1}", Common.DefaultModelDir() + "\\fp32\\skeleton-tracking.cubemos", ex));
                Console.ReadLine();
                return;
            }

初期化が終わったらモデルをロードします。
skeletontrackingApi.LoadModel一発でモデルのロードは完了します。引数ではモデルのあるパスを指定する他、ロード先のデバイスの指定を行います（CPU: 0 / GPU: 1 / Myriad: 2)。

EnableStream

           Pipeline pipeline = new Pipeline();
            Config cfg = new Config();
            Context context = new Intel.RealSense.Context();
            cfg.EnableStream(Intel.RealSense.Stream.Color, 1280, 720, Format.Bgr8, framerate : 30);
            PipelineProfile pp = pipeline.Start(cfg);

            int networkHeight = 128;

            using(var frames = pipeline.WaitForFrames())
            {
                var frame = frames.ColorFrame.DisposeWith(frames);
                System.Drawing.Bitmap inputImage =
                  new System.Drawing.Bitmap(frame.Width,
                                            frame.Height,
                                            frame.Stride,
                                            System.Drawing.Imaging.PixelFormat.Format24bppRgb,
                                            frame.Data);

次にRealSenseのライブラリを使ってカメラ映像をストリームします。RealSenseではPipelineを作成し、そこにカメラに応じた設定Contextをいれてstartすることでカメラ映像をデバイスに流し込むことができます。

cfg.EnableStreamメソッドでストリームするカメラの設定を定義します。第一引数となるカメラの定義はこのようになっています。

定義	内容
Any
Depth	深度データ
Color	カラーデータ
Infrared	赤外線データ
Fisheye	専用モーションカメラからキャプチャされた魚眼（ワイド）データ
Gyro	ジャイロスコープモーションデータ
Accel	加速度計モーションデータ
Gpio	GPIOを介して接続された外部デバイスからの信号
Pose	RealSenseで計算した6DoF（上下左右前後）の姿勢データ
Confidence	4ビット/ピクセル深度の信頼レベル

今回はColor、カラーデータをストリームします。

他にはストリームする幅と高さ、出力フォーマット（BGR8形式）、フレームレートを設定しています。

RealSenseにはフレームの取得方法としてWaitForFramesとPollForFramesの二つがあります。
WaitForFramesはフレームの読み込みが終わるまで次のフレームの読み込みを待つタイプ、PollForFramesは自分でフレームの読み込みを取りに行くタイプです。前者はカメラが一つの場合、後者は複数のカメラを扱うときに使ったりします。今回は前者を使います。

frameを取り出したらそこからbitmapを生成します。引数としてフレームの幅、高さ、ストライド、出力形式、格納するデータ配列のポインタを指定します。
これでカメラデータから画像を取得できました。

RunskeletonTracking

                System.Collections.Generic.List<Cubemos.SkeletonTracking.Api.SkeletonKeypoints> skeletonKeypoints;

               skeletontrackingApi.RunSkeletonTracking(ref inputImage, networkHeight, out skeletonKeypoints);

               Console.WriteLine("# Persons detected: " + skeletonKeypoints.Count);
                for (int skeleton_index = 0; skeleton_index < skeletonKeypoints.Count; skeleton_index++) {
                    var skeleton = skeletonKeypoints[skeleton_index];
                    Console.WriteLine("Skeleton #" + skeleton_index);
                    for (int joint_index = 0; joint_index < skeleton.listJoints.Count; joint_index++) {
                        Cubemos.SkeletonTracking.Api.Coordinate coordinate = skeleton.listJoints[joint_index];
                        Console.WriteLine("\tJoint coordinate #" + joint_index + ": " + coordinate.x + "; " +
                                          coordinate.y);
                    }
                }

最後は生成したBitmapデータに対して推論をかけます。これもRunSkeletonTracking一発でOKです。引数には対象となる画像、推論の結果として期待されるblobの高さ、出力先となる配列を指定します。

推論が終われば後はデータを並べて表示するだけです。それぞれの配列は関節のポイントとなるX、Y座標を表しています。

大体わかったところで、自分で似たようなプロジェクトを作ってみましょう。

自分でプロジェクトを作ってみる

次に同じ内容のプロジェクトを自分で作ってみたいと思います。
これも先日の記事内で環境設定と画像ファイルから推論を行うプログラムを書きましたので、そのプロジェクトを改変したいと思います（上記記事内「Getting Start(C#)」）。

まずはRealSenseのライブラリモジュールを追加します。右側のツリーから「参照」をクリックし「参照の追加」を選択します。右側にウインドウが出てきますので下部にある「参照」ボタンをクリックし Program Files\Cubemos\SkeletonTracking\bin\Intel.RealSense.dll を選択します。

追加が終わったらコードの改修です。直すポイントは

• 頭にIntel.RealSenseの参照を入れる
• Bitmapの作成前にカメラのストリームパイプラインを作成し、フレームをもってくる
• 人のカウント以外にキーポイントの座標も表示する

です。直したコードはこうなりました。

using System;
using Intel.RealSense;

/// 
/// This sample shows how to use the cubemos SkeletonTracking C# API in a console application /// 
/// 
namespace Cubemos.Samples
{
    class Program
    {
        static void Main(string[] args)
        {
            // Read an RGB image of any size 
            String cubemosSkelData = Environment.GetFolderPath(Environment.SpecialFolder.LocalApplicationData) + "\\Cubemos\\SkeletonTracking";
            //System.Drawing.Bitmap image = new System.Drawing.Bitmap(cubemosSkelData + "\\res\\images\\skeleton_estimation.jpg");

            // Decalre results container 
            System.Collections.Generic.List<Cubemos.SkeletonTracking.Api.SkeletonKeypoints> skeletonKeypoints;

            // Create cubemos API handle for Intel® Inference Plugin and specify the folder with the license key 
            var skeletontrackingApi = new Cubemos.SkeletonTracking.Api(cubemosSkelData + "\\license");

            // Load the CPU model 
            skeletontrackingApi.LoadModel(Cubemos.TargetComputeDevice.CM_CPU,
                cubemosSkelData + "\\models\\fp32\\skeleton-tracking.cubemos");
            Console.Write("Hold the Intel RealSense with person(s) in scene and hit <ENTER>... ");
            Console.ReadLine();

            // Initialise the intel realsense pipeline as an acquisition device
            Pipeline pipeline = new Pipeline();
            Config cfg = new Config();
            Context context = new Intel.RealSense.Context();
            cfg.EnableStream(Intel.RealSense.Stream.Color, 1280, 720, Format.Bgr8, framerate: 30);
            PipelineProfile pp = pipeline.Start(cfg);

            // Set the network input size to 128 for faster inference
            int networkHeight = 128;

            // Acquire a single color frame and run Skeleton Tracking on it
            using (var frames = pipeline.WaitForFrames())
            {
                var frame = frames.ColorFrame.DisposeWith(frames);
                System.Drawing.Bitmap inputImage =
                  new System.Drawing.Bitmap(frame.Width,
                                            frame.Height,

                                            frame.Stride,
                                            System.Drawing.Imaging.PixelFormat.Format24bppRgb,
                                            frame.Data);

                // Send inference request and get the poses 
                skeletontrackingApi.RunSkeletonTracking(ref inputImage, networkHeight, out skeletonKeypoints);

                // Output detected skeletons
                Console.WriteLine("# Persons detected: " + skeletonKeypoints.Count);
                for (int skeleton_index = 0; skeleton_index < skeletonKeypoints.Count; skeleton_index++)
                {
                    var skeleton = skeletonKeypoints[skeleton_index];
                    Console.WriteLine("Skeleton #" + skeleton_index);
                    for (int joint_index = 0; joint_index < skeleton.listJoints.Count; joint_index++)
                    {
                        Cubemos.SkeletonTracking.Api.Coordinate coordinate = skeleton.listJoints[joint_index];
                        Console.WriteLine("\tJoint coordinate #" + joint_index + ": " + coordinate.x + "; " +
                                          coordinate.y);
                    }
                }
            }
            Console.Write("\nPress any key to exit...");
            Console.ReadKey(true);
        }
    }
}

ビルドして実行した結果、サンプルと同じ動きをしたら成功です。

まとめ

今回は実際のRealSenseカメラを使って推論を行ってみました。C#は読みやすくていいですね。
次はPythonを試してみたいと思います。

参考サイト

• https://dev.intelrealsense.com/docs/api-architecture#section-high-level-pipeline-api
• https://github.com/IntelRealSense/librealsense/wiki/API-How-To#wait-for-coherent-set-of-frames
• https://docs.microsoft.com/ja-jp/dotnet/api/system.drawing.bitmap.-ctor?view=netframework-4.8#System_Drawing_Bitmap__ctor_System_Int32_System_Int32_System_Int32_System_Drawing_Imaging_PixelFormat_System_IntPtr_